#include <cmath>
#include <map>
#include <algorithm>
#include <mutex>
#include <iostream>
#include "props.h"

namespace eintik::property
{

int g_compress = 0;
double g_wave_speed = 0;
int g_cnt_points = 0;
int g_start_elmet = 0;
double g_gap_elmet = 0.0;
int g_end_elmet = 0;
double g_cscan_interval = 0;
int g_encoder_delay = 0;

int32_t g_encoder_resolution = 12;
int g_start_type = 0;
int g_beam_location = 0;
const std::string kWaveSpeed = "wave_speed";

std::mutex mtx_pos_1;

std::map<int, double> g_start_gate;
std::map<int, double> g_end_gate;
double g_threshold_gate_i = 0.5;
const double g_ratio_ascan_amplitude = 4096.0;
double g_wave_speed_gateuse = 0;
int g_compress_gateuse = 0;
std::mutex mtx_gate;

// start: mm
void set_gate_start(int id_gate, double start)
{
    std::lock_guard<std::mutex> lock(mtx_gate);

    if (id_gate < 0) {
        return;
    }
    if (start < 0) {
        return;
    }
    g_start_gate[id_gate] = start;
    // std::cout << "set_gate_start, id_gate: " << id_gate << ", start: " << start << std::endl;
}

// id of point,
int pos_gate_start(int id_gate)
{
    std::lock_guard<std::mutex> lock(mtx_gate);

    if (g_start_gate.find(id_gate) == g_start_gate.cend()) {
        return -1;
    }
    if (g_compress_gateuse == 0) {
        return -1;
    }
    if (g_wave_speed_gateuse == 0) {
        return -1;
    }
    // FIX ME: USB 10, PCIE 5,
    auto delta = 10 / 1000.0;
    auto id = std::min(
        static_cast<int>(g_start_gate[id_gate] * 2 / g_wave_speed_gateuse / (g_compress_gateuse * delta)),
        g_cnt_points - 1);
    // std::cout << "pos_gate_start, id_gate: " << id_gate << ", id: " << id << std::endl;
    return id;
}

// end: mm
void set_gate_end(int id_gate, double end)
{
    std::lock_guard<std::mutex> lock(mtx_gate);

    if (id_gate < 0) {
        return;
    }
    if (end < 0) {
        return;
    }
    g_end_gate[id_gate] = end;
    // std::cout << "set_gate_end, id_gate: " << id_gate << ", end: " << end << std::endl;
}

// id of point,
int pos_gate_end(int id_gate)
{
    std::lock_guard<std::mutex> lock(mtx_gate);

    if (g_end_gate.find(id_gate) == g_end_gate.cend()) {
        return -1;
    }
    if (g_compress_gateuse == 0) {
        return -1;
    }
    if (g_wave_speed_gateuse == 0) {
        return -1;
    }
    // FIX ME: USB 10, PCIE 5,
    auto delta = 10 / 1000.0;
    auto id = std::min(
        static_cast<int>(g_end_gate[id_gate] * 2 / g_wave_speed_gateuse / (g_compress_gateuse * delta)),
        g_cnt_points - 1);
    // std::cout << "pos_gate_end, id_gate: " << id_gate << ", id: " << id << std::endl;
    return id;
}

void set_threadhold_gate_i(double threshold)
{
    std::lock_guard<std::mutex> lock(mtx_gate);
    g_threshold_gate_i = threshold / 100.0;
}

double threadhold_gate_i()
{
    std::lock_guard<std::mutex> lock(mtx_gate);
    return g_threshold_gate_i;
}

bool pos_gate_isync(
    int id_gate, const int16_t *point, int cnt, 
    int &start, int &end, int *shift)
{
    std::lock_guard<std::mutex> lock(mtx_gate);

    if (g_end_gate.find(id_gate) == g_end_gate.cend()) {
        return false;
    }
    if (g_compress_gateuse == 0) {
        return -1;
    }
    if (g_wave_speed_gateuse == 0) {
        return -1;
    }

    int id_gate_i = -1;
    for (auto i = 0; i < cnt - 1; ++i) {
        auto a = std::abs(point[i]) / g_ratio_ascan_amplitude;
        auto b = std::abs(point[i + 1]) / g_ratio_ascan_amplitude;
        if (a < g_threshold_gate_i && b > g_threshold_gate_i) {
            id_gate_i = i + 1;
            break;
        }
    }
    if (-1 == id_gate_i || id_gate_i >= cnt) {
        return false;
    }
    // FIX ME: USB 10, PCIE 5,
    auto delta = 10 / 1000.0;
    int id_shift = g_start_gate[id_gate] * 2 / g_wave_speed_gateuse / (g_compress_gateuse * delta);
    auto start_0 = id_gate_i + id_shift;
    if (start_0 < 0) {
        start_0 = 0;
    }
    if (start_0 >= cnt) {
        start_0 = cnt - 1;
    }
    id_shift = g_end_gate[id_gate] * 2 / g_wave_speed_gateuse / (g_compress_gateuse * delta);
    auto end_0 = id_gate_i + id_shift;
    if (end_0 < 0) {
        end_0 = 0;
    }
    if (end_0 >= cnt) {
        end_0 = cnt - 1;
    }
    if (start_0 > end_0) {
        return false;
    }
    start = start_0;
    end = end_0;
    if (shift) {
        *shift = id_gate_i;
    }
    return true;
}

std::pair<int, double> on_point(
    double range_start, double range_end, double velocity, int cnt_points_max)
{
    mtx_pos_1.lock();
    auto cnt_points = 0;
    auto compress = 1;
    auto time_consumed = (range_end - range_start) * 2 / (velocity / 1000);
    // FIX ME: USB 10, PCIE 5,
    auto delta = 10;
    for (auto i = 1;; i++) {
        auto points = time_consumed * 1000 / (i * delta);
        cnt_points = std::ceil(points);
        cnt_points = (cnt_points / 8 + (cnt_points % 8 ? 1 : 0)) * 8;
        if (cnt_points_max >= cnt_points) {
            compress = i;
            break;
        }
    }
    auto time_range_valid = compress * delta / 1000.0 * cnt_points / 2;
    g_compress = compress;
    g_wave_speed = velocity / 1000;
    g_cnt_points = cnt_points;
    mtx_pos_1.unlock();

    mtx_gate.lock();
    g_wave_speed_gateuse = g_wave_speed;
    g_compress_gateuse = g_compress;
    mtx_gate.unlock();
    
    return std::pair{cnt_points, time_range_valid};
}

double delay_time(double range_start)
{
    return range_start / 1.48;
}

double signal_range()
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    // FIX ME: USB 10, PCIE 5,
    auto delta = 10 / 1000.0;
    auto range = (g_compress * g_cnt_points * g_wave_speed * delta) / 2;

    return range;
}

double gap_range()
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    auto gap = (g_end_elmet - g_start_elmet - 1) * g_gap_elmet;
    return gap;
}

void set_element_start(int start)
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    g_start_elmet = start;

}

void set_element_end(int end)
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    g_end_elmet = end;
}

void set_gap_element(double gap)
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    g_gap_elmet = gap;
}

double pos_gap_element()
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    return g_gap_elmet;
}

void set_cscan_interval(double cscan_interval)
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    g_cscan_interval = cscan_interval;
}

double cscan_interval()
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    return g_cscan_interval;
}

int cnt_points()
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    return g_cnt_points;
}

void set_encoder_delay(int encoder_delay)
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    g_encoder_delay = encoder_delay;
}

int encoder_delay()
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    return g_encoder_delay;
}

int cnt_beam()
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    return g_end_elmet - g_start_elmet + 1;
}

int32_t encoder_resolution()
{
    #if 0
    *** 0916: time encoder,
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    return g_encoder_resolution;
    #endif
    
    return 1;
}

void set_beam_location(int beam_location)
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    g_beam_location = beam_location;
}

int beam_location()
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    return g_beam_location;
}

void set_start_type(int type)
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    g_start_type = type;
}

int start_type()
{
    std::lock_guard<std::mutex> lock(mtx_pos_1);
    return g_start_type;
}

}
